/*
 * selectComponent.cpp
 *
 *  Created on: 06/ago/2012
 *      Author: alby
 */

#include "selectComponent.h"
#include <vector>

using namespace std;

pcl::PointCloud<pcl::PointXYZRGB>::Ptr component_selected (new pcl::PointCloud<pcl::PointXYZRGB>);

pcl::PointCloud<pcl::PointXYZRGB>::Ptr ribbon_cable (new pcl::PointCloud<pcl::PointXYZRGB>);

pcl::PointCloud<pcl::PointXYZRGB>::Ptr getSelectedComponent(){

	return component_selected;

}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr getRibbonCable(){
	return ribbon_cable;
}

void segmentCluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, pcl::PointIndices::Ptr output, int selectedPointIndex, double threshold, bool ribbon){

	threshold = 1;
	vector<pcl::PointIndices> cluster_indices_out;

	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
	ec.setClusterTolerance (threshold);
	ec.setMinClusterSize (100);
	ec.setMaxClusterSize (input->size());
	ec.setInputCloud (input);
	ec.extract (cluster_indices_out);

	//    i cluster index
	//    j index of the point in the i-th cluster
	//    if the j-th point is the one selected the cluster selected is the i-th

	int selectedClusterIndex=-1;
	for (int i=0;i<cluster_indices_out.size();i++)
		if (selectedClusterIndex>0) break;
		else
			for (int j=0;j<cluster_indices_out[i].indices.size();j++)
				  if(cluster_indices_out[i].indices.at(j)==selectedPointIndex){
					  selectedClusterIndex=i;
					  break;
				  }

	pcl::PointIndices temp = cluster_indices_out[selectedClusterIndex];

	pcl::PointXYZRGB clickedPoint = (*input)[selectedPointIndex];

	//pcl::PointCloud<pcl::PointXYZRGB>::Ptr without_component (new pcl::PointCloud<pcl::PointXYZRGB>);
	//pcl::copyPointCloud(*(input),*(without_component));


	if(ribbon)
		ribbon_cable->clear();

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

	while(!temp.indices.empty()){
		//without_component->erase(without_component->begin() + temp.indices.back());
		if(ribbon)
			output_cloud->push_back(input->at(temp.indices.back()));
		output->indices.push_back(temp.indices.back());
		temp.indices.pop_back();
	}

	//pcl::io::savePCDFile("ciao.pcd", *without_component);
	if(ribbon)
		pcl::copyPointCloud(*(output_cloud),*(ribbon_cable));

}


void colorCluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, pcl::PointIndices::Ptr indices, double r, double g, double b){

	component_selected->clear();

	while (! indices->indices.empty()){

		int pointN= indices->indices.back();
		component_selected->push_back(input->at(pointN));
		input->at(pointN).r = r;
		input->at(pointN).g = g;
		input->at(pointN).b = b;
		indices->indices.pop_back();

	}

}


bool findCluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr target, pcl::PointCloud<pcl::PointXYZRGB>::Ptr source, pcl::PointIndices::Ptr outIndices, int selectedPointIndex, double threshold ){

	threshold = 1;
	vector<pcl::PointIndices> cluster_indices_out;

	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
	ec.setClusterTolerance (threshold);
	ec.setMinClusterSize (100);
	ec.setMaxClusterSize (target->size());
	ec.setInputCloud (target);
	ec.extract (cluster_indices_out);


	//TROVO IL CLUSTER CHE CONTIENE IL PUNTO CHE HO SELEZIONATO CON IL MOUSE
	int selectedClusterIndex=-1;
	for (int i=0;i<cluster_indices_out.size();i++)
		if (selectedClusterIndex>0) break;
		else
			for (int j=0;j<cluster_indices_out[i].indices.size();j++)
				  if(cluster_indices_out[i].indices.at(j)==selectedPointIndex){
					  selectedClusterIndex=i;
					  break;
				  }

	pcl::PointIndices temp = cluster_indices_out[selectedClusterIndex];

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);

	//CREO LA POINT CLOUD DEL CLUSTER D'INTERESSE
	while(!temp.indices.empty()){
		int pointN= temp.indices.back();
		output->push_back(target->at(pointN));
		temp.indices.pop_back();
	}

	//CALCOLO IL CENTROIDE DELLA POINT CLOUD RAPPRESENTANTE IL COMPONENTE SELEZIONATO
	Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*output,centroid);
	float *data = centroid.data();

	vector<pcl::PointIndices> cluster_indices_out2;
	double dim = output->size();

	//TROVO I POSSIBILI CLUSTER CANDIDATI NELLA SORGENTE
	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec2;
	ec2.setClusterTolerance (threshold);
	ec2.setMinClusterSize (dim - ceil((0.15)*(dim)));
	ec2.setMaxClusterSize (dim + ceil((0.15)*(dim)));
	ec2.setInputCloud (source);
	ec2.extract (cluster_indices_out2);

	cout<<"Numero di candidati: "<<cluster_indices_out2.size()<<endl;

	for (int i = 0; i < cluster_indices_out2.size(); i++){

		pcl::PointIndices temp2 = cluster_indices_out2[i];
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr output2 (new pcl::PointCloud<pcl::PointXYZRGB>);

		outIndices->indices.clear();

		while(!temp2.indices.empty()){
			int pointN= temp2.indices.back();
			output2->push_back(source->at(pointN));
			outIndices->indices.push_back(temp2.indices.back());
			temp2.indices.pop_back();
		}

		Eigen::Vector4f centroid2;
		pcl::compute3DCentroid(*output2, centroid2 );
		float *data2 = centroid2.data();

		//Soglia dinamica, calcolata basandosi su valori sperimentali.

		double distance;
		if (output->points.size() > (0.05*target->points.size())){
			distance = output->points.size()*0.0000843;
			cout<<"Moltiplicatore: "<<0.0000843<<endl;
		}
		else{
			distance = output->points.size()*0.0002543; //0.0001043
			cout<<"Moltiplicatore: "<<0.0002543<<endl;
		}

		cout<<"Soglia: "<<distance<<endl;

		cout<<"differenza coordinate x: "<< data[0] - data2[0] <<" y: "<< data[1] - data2[1]<<" z: "<< data[2] - data2[2]<<endl;

		distance = 1;

		if(abs(data[0] - data2[0]) < distance &&
			abs(data[1] - data2[1]) < distance &&
			abs(data[2] - data2[2]) < distance ){

			cout<<"centroide trovato x: "<< data[0] - data2[0] <<" y: "<< data[1] - data2[1]<<" z: "<< data[2] - data2[2]<<endl;
			//cout<<"Numero punti: "<<cluster_indices_out2[i].indices.size()<<endl;
			return true;
		}

	}

	return false;

}


bool isRibbonCableCorrect(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source){

	vector<pcl::PointIndices> cluster_indices_out2;
	double dim = ribbon_cable->points.size();

	//TROVO I POSSIBILI CLUSTER CANDIDATI NELLA SORGENTE
	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec2;
	ec2.setClusterTolerance (2);
	ec2.setMinClusterSize (dim - ceil((0.25)*(dim)));
	ec2.setMaxClusterSize (dim + ceil((0.25)*(dim)));
	ec2.setInputCloud (source);
	ec2.extract (cluster_indices_out2);

	cout<<"Numero di candidati: "<<cluster_indices_out2.size()<<endl;

	for (int i = 0; i < cluster_indices_out2.size(); i++){

		pcl::PointIndices temp = cluster_indices_out2[i];
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr output2 (new pcl::PointCloud<pcl::PointXYZRGB>);

		while(!temp.indices.empty()){
			int pointN= temp.indices.back();
			output2->push_back(source->at(pointN));
			temp.indices.pop_back();
		}

		//TARGET RIBBON CABLE CENTROID
		Eigen::Vector4f centroid;
		pcl::compute3DCentroid(*ribbon_cable, centroid );
		float *data = centroid.data();

		//SOURCE RIBBON CABLE CENTROID
		Eigen::Vector4f centroid2;
		pcl::compute3DCentroid(*output2, centroid2 );
		float *data2 = centroid2.data();


		if(abs(data[0] - data2[0]) < 1 &&
			abs(data[1] - data2[1]) < 1 &&
			abs(data[2] - data2[2]) < 1 ){

			cout<<"centroide trovato x: "<< data[0] - data2[0] <<" y: "<< data[1] - data2[1]<<" z: "<< data[2] - data2[2]<<endl;
			return true;
		}

	}

	return false;

}
